#include <fstream>
#include <stdlib.h>
#include <math.h>

#include "Matrix.hpp"

using std::ifstream;
using std::ostream;

Matrix::Matrix( int n, int m ) : _n( n ), _m( m )
{
  _values = new float[ _n*_m ];
}

void Matrix::print( ostream& out )
{
  
  out << _n << ' ' << _m << '\n';
  
  for( int i=0; i<_n; i++ )
  {
    for( int j=0; j<_m; j++ )
      out << _values[ i*_m+j ] << ' ';
    out << '\n';
  }
  
}

void Matrix::clear()
{
  
  delete _values;
  
}

Matrix Matrix::generate( int size )
{
  
  Matrix lMatrix = generateL( size );
  Matrix uMatrix = generateU( size );
  
  Matrix ret = lMatrix.mul( uMatrix );
  
  lMatrix.clear();
  uMatrix.clear();
  
  return ret;
  
}

Matrix Matrix::generateL( int size )
{
  
  Matrix ret( size, size );
  
  for( int i=0; i<size; i++ )
    for( int j=0; j<size; j++ )
    {
      if( i<j )
	ret._values[ i*size+j ] = 0;
      else if( i==j )
	ret._values[ i*size+j ] = 1;
      else
        ret._values[ i*size+j ] = static_cast< float >( rand() )/RAND_MAX*20 - 10;
    }
    
 return ret;
  
}

Matrix Matrix::generateU( int size )
{
  
  Matrix ret( size, size );
  
  for( int i=0; i<size; i++ )
    for( int j=0; j<size; j++ )
    {
      
      if( i<j )
        ret._values[ i*size+j ] = static_cast< float >( rand() )/RAND_MAX*20-10;
      else if( i==j )
      {
	float value = static_cast< float >( rand() )/RAND_MAX*20-10;
	if( fabs( value ) < 1e-2 )
	  value += 1e-1 * ( value>=0 ? 1 : -1 );
	
	ret._values[ i*size+j ] = value;
      }
      else
	ret._values[ i*size+j ] = 0;
      
    }
  
  return ret;
  
}

Matrix Matrix::generateIdentity( int size )
{
  
  Matrix ret( size, size );
  
  for( int i=0; i<size; i++ )
    for( int j=0; j<size; j++ )
    {
      if( i==j )
	ret._values[ i*size+j ] = 1;
      else
	ret._values[ i*size+j ] = 0;
    }
    
  return ret;
  
}

Matrix Matrix::load( const char* filename )
{
  
  ifstream in( filename );
  
  int n, m;
  
  in >> n >> m;
  Matrix ret( n, m );
  
  for( int i=0; i<n; i++ )
    for( int j=0; j<m; j++ )
      in >> ret._values[ i*m+j ];
    
  return ret;
  
}

Matrix Matrix::mul( Matrix matrix )
{
  
  Matrix ret( _n, matrix._m );
  mul( matrix, &ret );
  
  return ret;
  
}

void Matrix::mul( Matrix matrix, Matrix *ret )
{
  
  for( int i=0; i< _n; i++ )
    for( int j=0; j< matrix._m; j++ )
    {
      float value = 0.0;
      
      for( int k=0; k<_m; k++ )
	value += _values[ i*_m + k ] * matrix._values[ k*matrix._m + j ];
      
      ret->_values[ i*matrix._m + j ] = value;
    }
  
}

Matrix Matrix::copy()
{
  Matrix ret( _n, _m );
  copy( &ret );
  
  return ret;
}

void Matrix::copy( Matrix *result, int yOffset, int xOffset )
{
  
  for( int i=yOffset; i<_n+yOffset; i++ )
    for( int j=xOffset; j<_m+xOffset; j++ )
      result->_values[ i*result->_m + j ] = _values[ (i-yOffset)*_m + (j-xOffset) ];
  
}

Matrix Matrix::submatrix( int xOffset, int yOffset, int xSize, int ySize )
{
  
  Matrix ret( ySize, xSize );
  
  for( int i=0; i<ySize; i++ )
    for( int j=0; j<xSize; j++ )
      ret._values[ i*xSize + j ] = _values[ (i+yOffset)*_m + j+xOffset ];
  
 return ret;
    
}

int Matrix::N()
{
  
  return _n;
  
}

void Matrix::doLUStep( Matrix lMatrix, int baseLine )
{
 
  for( int i=baseLine+1; i<_n; i++ )
  {
    float val = _values[ i*_m+baseLine ] / _values[ baseLine*_m+baseLine ];
    
    lMatrix._values[ i*_m+baseLine ] = val;
    
    for( int j=baseLine; j<_m; j++ )
      _values[ i*_m+j ] -= val*_values[ baseLine*_m+j ];
  }
  
  
}

void Matrix::doLU( Matrix lMatrix )
{
  
  for( int i=0; i<_n; i++ )
    doLUStep( lMatrix, i );
  
}

const int blockSize = 2;

void Matrix::doLUBlock( Matrix lMatrix )
{
  
  for( int i=0; i<_n; i+= blockSize )
    doLUBlockStep( lMatrix, i );
  
}

void Matrix::doLUBlockStep( Matrix lMatrix, int offset )
{
  
  Matrix subU = submatrix( offset, offset, blockSize, blockSize );
  Matrix subL = generateIdentity( blockSize );
  
  subU.doLU( subL );
  
  subU.copy( this, offset, offset );
  subL.copy( &lMatrix, offset, offset );
  
  if( offset+blockSize == _n )
    return;
  
  Matrix u2 = submatrix( offset+blockSize, offset, _m-offset-blockSize, blockSize );
  u2.solveSystemRight( subL );
  u2.copy( this, offset, offset+blockSize );
  
  Matrix l2 = submatrix( offset, offset+blockSize, blockSize, _n-offset-blockSize );
  l2.solveSystemLeft( subU );
  l2.copy( &lMatrix, offset+blockSize, offset );
  
  Matrix lu12 = l2.mul( u2 );
  for( int i=offset+blockSize; i<_n; i++ )
    for( int j=offset+blockSize; j<_m; j++ )
      _values[ i*_m + j ] -= lu12._values[ (i-offset-blockSize)*lu12._m + (j-offset-blockSize) ];
  
  subL.clear();
  subU.clear();
  l2.clear();
  u2.clear();
  lu12.clear();
  
  for( int i=offset+blockSize; i<_n; i++ )
    for( int j=offset; j<offset+blockSize; j++ )
      _values[ i*_m + j ] = 0;
  
}

void Matrix::solveSystemRight( Matrix lMatrix )
{
  for( int k=0; k<_n-1; k++ )
    for( int i=k+1; i<_n; i++ )
    {
    
      float value = lMatrix._values[ i*lMatrix._m + k ];
    
      for( int j=0; j<_m; j++ )
        _values[ i*_m+j ] -= _values[ k*_m + j ]*value;
    }
}

void Matrix::solveSystemLeft( Matrix uMatrix )
{
  
  for( int i=0; i<_m; i++ )
    solveSystemLeftStep( uMatrix, i );
  
}

void Matrix::solveSystemLeftStep( Matrix uMatrix, int offset )
{
  
  for( int i=0; i<_n; i++ )
    _values[ i*_m + offset ] /= uMatrix._values[ offset*uMatrix._m + offset ];
  
  for( int i=offset+1; i<_m; i++ )
  {
    float value = uMatrix._values[ offset*_m + i ];
    
    for( int j=0; j<_n; j++ )
      _values[ j*_m+i ] -= _values[ j*_m + offset ]*value;
    
    
  }
}